/*
 * algebraBasic.cpp
 *
 *  Created on: 27-nov.-2013
 *      Author: Axel
 */

#include "algebraBasic.h"

#include "extraMath.h"
#include <GL/glew.h>
#include <glfw3.h>

//extra functions:
Vector3f mixLine(Vector3f a, Vector3f b, float t) {
	Vector3f ret;
	ret.x = a.x * (1-t) + b.x * t;
	ret.y = a.y * (1-t) + b.y * t;
	ret.z = a.z * (1-t) + b.z * t;

	return ret;
}


Vector4f mix(Vector4f A, Vector4f B, float t) {
    //return B + t * (B - A);
	Vector4f ret;

	ret.x = A.x * (1-t) + B.x * t;
	ret.y = A.y * (1-t) + B.y * t;
	ret.z = A.z * (1-t) + B.z * t;
	ret.w = A.w * (1-t) + B.w * t;

	return ret;
}

Vector4f quadratic_bezier(Vector4f A, Vector4f B, Vector4f C, float t) {
	Vector4f D = mix(A, B, t);      // D = A + t(B - A)
	Vector4f F  = mix(B, C, t);		// E = B + t(C - B)
	Vector4f P = mix(D, F, t);      // P = D + t(E - D)

    return P;
}

Vector4f cubic_bezier(Vector4f A, Vector4f B, Vector4f C, Vector4f D, float t) {
	Vector4f H = mix(A, B, t);      // E = A + t(B - A)
	Vector4f F = mix(B, C, t);      // F = B + t(C - B)
	Vector4f G = mix(C, D, t);      // G = C + t(D - C)

    return quadratic_bezier(H, F, G, t);
}

Vector4f quintic_bezier(Vector4f A, Vector4f B, Vector4f C, Vector4f D, Vector4f J, float t) {
	Vector4f F = mix(A, B, t);      // F = A + t(B - A)
	Vector4f G = mix(B, C, t);      // G = B + t(C - B)
	Vector4f H = mix(C, D, t);      // H = C + t(D - C)
	Vector4f I = mix(D, J, t);      // I = D + t(E - D)

    return cubic_bezier(F, G, H, I, t);
}

Vector4f cubic_bspline_10(Vector4f CP[10], float t) {
    float f = t * 3.0;
    int i = int(math::round_axel(f));
    float s = t;

    if (t <= 0.0)
        return CP[0];

    if (t >= 1.0)
        return CP[9];

    Vector4f A = CP[i * 3];
    Vector4f B = CP[i * 3 + 1];
    Vector4f C = CP[i * 3 + 2];
    Vector4f D = CP[i * 3 + 3];

    return cubic_bezier(A, B, C, D, s);
}

//Vectors 4f:
float Vector4f::operator*(const Vector4f& Right) const {
	float dot = Right.x*x + Right.y*y + Right.z*z + Right.w*w;
	return dot;
}

Vector4f Vector4f::operator*(const float& right) const {
	Vector4f ret;
	ret.x = right * x;
	ret.y = right * y;
	ret.z = right * z;
	ret.w = right * w;
	return ret;
}

Vector4f Vector4f::operator+(const Vector4f& Right) const {
	Vector4f ret;
	ret.x = Right.x + x;
	ret.y = Right.y + y;
	ret.z = Right.z + z;
	ret.w = Right.w + w;
	return ret;
}

Vector4f Vector4f::operator-(const Vector4f& Right) const {
	Vector4f ret;
	ret.x = - Right.x + x;
	ret.y = - Right.y + y;
	ret.z = - Right.z + z;
	ret.w = - Right.w + z;
	return ret;
}

Vector4f Vector4f::operator-() const {
	Vector4f ret;
	ret.x = - x;
	ret.y = - y;
	ret.z = - z;
	ret.w = - w;
	return ret;
}

Vector4f& Vector4f::operator+=(const Vector4f& right) {
	this->x += right.x;
	this->y += right.y;
	this->z += right.z;
	this->w += right.w;
	return *this;
}

Vector4f& Vector4f::operator-=(const Vector4f& right) {
	this->x -= right.x;
	this->y -= right.y;
	this->z -= right.z;
	this->w -= right.w;
	return *this;
}

long Vector4f::length() {
	return math::root_axel((x*x+y*y+z*z+w*w),2);
}

//Vectors 3f:
float Vector3f::operator*(const Vector3f& Right) const {
	float dot = Right.x*x + Right.y*y + Right.z*z;
	return dot;
}

Vector3f Vector3f::operator+(const Vector3f& Right) const {
	Vector3f ret;
	ret.x = Right.x + x;
	ret.y = Right.y + y;
	ret.z = Right.z + z;
	return ret;
}

Vector3f Vector3f::operator-(const Vector3f& Right) const {
	Vector3f ret;
	ret.x = - Right.x + x;
	ret.y = - Right.y + y;
	ret.z = - Right.z + z;
	return ret;
}

Vector3f Vector3f::operator-() const {
	Vector3f ret;
	ret.x = - x;
	ret.y = - y;
	ret.z = - z;
	return ret;
}

Vector3f& Vector3f::operator+=(const Vector3f& right) {
	this->x += right.x;
	this->y += right.y;
	this->z += right.z;
	return *this;
}

Vector3f& Vector3f::operator-=(const Vector3f& right) {
	this->x -= right.x;
	this->y -= right.y;
	this->z -= right.z;
	return *this;
}

Vector3f Vector3f::crossProd(Vector3f right) {
	Vector3f cross;

	cross.x = y*right.z - z*right.y;
	cross.y = z*right.x - x*right.z;
	cross.z = x*right.y - y*right.x;

	return cross;
}

long Vector3f::length() {
	return math::root_axel((x*x+y*y+z*z),2);
}

bool Vector3f::operator <(Vector3f& right) {
	return length()<right.length();
}

bool Vector3f::operator >(Vector3f& right) {
	return length()>right.length();
}

bool Vector3f::operator <=(Vector3f& right) {
	return length()<=right.length();
}

bool Vector3f::operator >=(Vector3f& right) {
	return length()>+right.length();
}


//matrix 4f:
void MatrixNMf::InitIdentity() {
	for(int i=0;i<row;i++)
		for(int j=0;j<col;j++) {
			if (i != j)
				m[i][j] = 0.0f;
			else
				m[i][j] = 1.0f;
		}
}

MatrixNMf MatrixNMf::operator*(const MatrixNMf& Right) const {
	MatrixNMf Ret(row, Right.col);

	if (col == Right.row) {
		for (int i = 0; i < row; i++)
			for (int j = 0; j < Right.col; j++)
				for (int a = 0; a < col; a++)
					Ret.m[i][j] += m[i][a] * Right.m[a][j];
	}

	return Ret;
}

MatrixNMf MatrixNMf::operator+(const MatrixNMf& Right) const {
	MatrixNMf Ret(row, col);

	if (row == Right.row && col == Right.col) {

		for (int i = 0; i < row; i++) {
			for (int j = 0; j < col; j++) {
				Ret.m[i][j] = m[i][j] + Right.m[i][j];
			}
		}
	}
	return Ret;
}

MatrixNMf MatrixNMf::operator-(const MatrixNMf& Right) const {
	MatrixNMf Ret(row, col);

	if (row == Right.row && col == Right.col) {

		for (int i = 0; i < row; i++) {
			for (int j = 0; j < col; j++) {
				Ret.m[i][j] = m[i][j] - Right.m[i][j];
			}
		}
	}
	return Ret;
}

MatrixNMf MatrixNMf::operator-() const {
	MatrixNMf Ret(row, col);

	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			Ret.m[i][j] = - m[i][j];
		}
	}

	return Ret;
}

MatrixNMf& MatrixNMf::operator+=(const MatrixNMf& Right) {
	if (row == Right.row && col == Right.col) {

		for (int i = 0; i < row; i++) {
			for (int j = 0; j < col; j++) {
				m[i][j] += Right.m[i][j];
			}
		}
	}
   return *this;
}

MatrixNMf& MatrixNMf::operator-=(const MatrixNMf& Right) {
	if (row == Right.row && col == Right.col) {

		for (int i = 0; i < row; i++) {
			for (int j = 0; j < col; j++) {
				m[i][j] -= Right.m[i][j];
			}
		}
	}
	return *this;
}

//matrix:
void Matrix4f::InitIdentity() {
   m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f;
   m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = 0.0f;
   m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = 0.0f;
   m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f;
}

Vector3f Matrix4f::matVector(Vector3f vec) {
	Vector3f out;
	out.x = m[0][0] * vec.x + m[0][1] * vec.y + m[0][2] * vec.z + m[0][3] * 1;
	out.y = m[1][0] * vec.x + m[1][1] * vec.y + m[1][2] * vec.z + m[1][3] * 1;
	out.z = m[2][0] * vec.x + m[2][1] * vec.y + m[2][2] * vec.z + m[2][3] * 1;
	return out;
}

bool Matrix4f::operator==(const Matrix4f& Right) const {
	bool same = true;

	for(int i=0;i<4;i++)
		for(int j=0;j<4;j++)
			if (m[i][j] != Right.m[i][j]) {
				same = false;
				break;
			}

	return same;
}

Matrix4f Matrix4f::operator*(const Matrix4f& Right) const {
   Matrix4f Ret;

   for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   Ret.m[i][j] = m[i][0] * Right.m[0][j] +
						 m[i][1] * Right.m[1][j] +
						 m[i][2] * Right.m[2][j] +
						 m[i][3] * Right.m[3][j];
	   }
   }

   return Ret;
}

Matrix4f Matrix4f::operator+(const Matrix4f& Right) const {
   Matrix4f Ret;

   for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   Ret.m[i][j] = m[i][j] + Right.m[i][j];
	   }
   }

   return Ret;
}

Matrix4f Matrix4f::operator-(const Matrix4f& Right) const {
   Matrix4f Ret;

   for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   Ret.m[i][j] = m[i][j] - Right.m[i][j];
	   }
   }

   return Ret;
}

Matrix4f Matrix4f::operator-() const {
   Matrix4f Ret;

   for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   Ret.m[i][j] = - m[i][j];
	   }
   }

   return Ret;
}

Matrix4f& Matrix4f::operator+=(const Matrix4f& Right) {
	for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   m[i][j] -= Right.m[i][j];
	   }
   }

   return *this;
}

Matrix4f& Matrix4f::operator-=(const Matrix4f& Right) {
   for (unsigned int i = 0 ; i < 4 ; i++) {
	   for (unsigned int j = 0 ; j < 4 ; j++) {
		   m[i][j] += Right.m[i][j];
	   }
   }
   return *this;
}

//Quaternions:
Quaternion Quaternion::operator*(const Quaternion& right) const {
	Quaternion ret;

	ret.x = x*right.x - y*right.y - z*right.z - w*right.w;
	ret.y = x*right.y + y*right.x +z*right.w - w*right.x;
	ret.z = x*right.z - y*right.w + z*right.x + w*right.y;
	ret.w = x*right.w + y*right.z - z*right.y + w*right.x;

	return ret;
}


Quaternion Quaternion::operator+(const Quaternion& right) const {
	Quaternion ret;

	ret.x = x + right.x;
	ret.y = y + right.y;
	ret.z = z + right.z;
	ret.w = w + right.w;

	return ret;
}

Quaternion Quaternion::operator-(const Quaternion& right) const {
	Quaternion ret;

	ret.x = x - right.x;
	ret.y = y - right.y;
	ret.z = z - right.z;
	ret.w = w - right.w;

	return ret;
}

Quaternion Quaternion::operator-() const {
	Quaternion ret;

	ret.x = -x;
	ret.y = -y;
	ret.z = -z;
	ret.w = -w;

	return ret;
}

Quaternion& Quaternion::operator+=(const Quaternion& right) {
	x += right.x;
	y += right.y;
	z += right.z;
	w += right.w;

	return *this;
}

Quaternion& Quaternion::operator-=(const Quaternion& right) {
	x -= right.x;
	y -= right.y;
	z -= right.z;
	w -= right.w;

	return *this;
}

double Quaternion::magnitude() {
	return math::root_axel((x*x+y*y+z*z+w*w),2);
}
